/*! @file missions/main.cpp
 *  @version 4.0.0
 *  @date Jun 05 2017
 *
 *  @brief
 *  main for GPS Missions API usage in a Linux environment.
 *  Shows example usage of the Waypoint Missions and Hotpoint Missions through
 *  the
 *  Mission Manager API.
 *
 *  @Copyright (c) 2017 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "mission_sample.hpp"
#include "flight_sample.hpp"
#include "dji_linux_helpers.hpp"

//lry
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <json/json.h>
#include <string>
#include <vector>
#include <queue>

// 多线程
#include <thread>
#include <mutex>
#include <condition_variable>
#include <unistd.h>
#include <sys/select.h>
#include <atomic>

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;


#define BUFFER_SIZE 1024
// global 
std::queue<std::vector<double>> task;
std::mutex mutex_;
std::mutex mutex_cmd_;
std::condition_variable cond_;
std::atomic<bool> net_done(false);
//std::lock_guard<std::mutex> lock(mutex_);
int task_cnt = 1;


enum CMD  {
  none = 0,
  gohome,
  ascend,
  descend
};

CMD g_cmd = none;

static void sleep_ms(unsigned int secs) {
	struct timeval tval;
	tval.tv_usec = (secs*1000)%1000000;
	select(0, NULL, NULL, NULL, &tval);
}

// 命令拆分，防止粘包
vector<string> parse(const string &s) {
    vector<string> res;
    int sz = s.size();

    size_t start = 0;
    for (size_t i = 0; i < sz; ++ i) {
        if (s[i] == '{') {
            start = i;
        }else if (s[i] == '}') {
            res.push_back(s.substr(start, i - start + 1));
        }
    }
    return res;
}

void netThread();


int
main(int argc, char** argv)
{
  sleep(6);  // 等待无人机初始化
  // Initialize variables
  int functionTimeout = 1;

  // Setup OSDK.
  LinuxSetup linuxEnvironment(argc, argv);
  Vehicle*   vehicle = linuxEnvironment.getVehicle();
  if (vehicle == NULL)
  {
    std::cout << "Vehicle not initialized, exiting.\n";
    return -1;
  }

  // lry net thread
  std::thread net(netThread);


  // Obtain Control Authority
  vehicle->control->obtainCtrlAuthority(functionTimeout);
  // lry
  FlightSample* flightSample = new FlightSample(vehicle);
  // flightSample->monitoredTakeoff();
  // vehicle->flightController->setCollisionAvoidanceEnabledSync(
  //         FlightController::AvoidEnable::AVOID_ENABLE, 1);


  
  // Setup variables for use
  uint8_t wayptPolygonSides;
  int     hotptInitRadius;
  int     responseTimeout = 1;

  // test 
  //   teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout);
  //   std::cout << "finish teardownSubscription...\n";
  // flightSample->moveByPositionOffset((FlightSample::Vector3f){7, 0, 10}, 0);
  // flightSample->setGoHomeAltitude(5);
  // vehicle->flightController->setCollisionAvoidanceEnabledSync(
  //       FlightController::AvoidEnable::AVOID_DISABLE, 1);



  // set homepoint
  if (!setUpSubscription(vehicle, responseTimeout))
  {
    std::cout << "Failed to set up Subscription!" << std::endl;
    return false;
  }
  sleep(1);
  // 获取的经纬度由 弧度表示 
  Telemetry::TypeMap<TOPIC_GPS_FUSED>::type homepoint;  
  homepoint = vehicle->subscribe->getValue<TOPIC_GPS_FUSED>();  // longitude , latitude , altitude
  std::cout << "\n\n initialize homepoint at :                " << "( " << 
      radians2degree(homepoint.longitude) << ',' << radians2degree(homepoint.latitude) << ',' << homepoint.altitude << " )\n";
  // teardown
  std::cout << "start teardownSubscription..." << std::endl;
  teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout);
  std::cout << "finish teardownSubscription...\n";
/*

// test
  task.push({2.00001, 1.99999, 20});
  task.push({2.00010, 2.00000, 30});
  task.push({2.00020, 1.99999, 30}); 
  task.push({2.00001, 1.99999, 10});
  task.push({2.00010, 2.00000, 20});
  task.push({2.00020, 1.99999, 30}); 
*/


/*   // task cmd format
    {
        cmd_type, {0, 1, 2, 3} -> {默认， 返航， 上升， 下降}
        long, 
        lat, 
        re_alt, 
        homepoint_alt
    }
*/

  // lry set up subscription before run waypointmission!
/*
  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    if (!setUpSubscription(vehicle, responseTimeout))
    {
      std::cout << "Failed to set up Subscription!" << std::endl;
      return false;
    }
    sleep(1);
  }
*/
  // lry
  vector<vector<double>> t;  // (n, 5);
  CMD cur_cmd = none;

  while (true) { // 可能阻塞在等网络指令
      if (net_done) {
          std::thread net(netThread);  // 重启网络线程
          net_done = false;
      }
      t.clear();
      // bool flag = true;  // has data
      //std::cout << "try to get task...\n";
      {  // critical zone
          std::unique_lock<std::mutex> lock(mutex_);
          // 考虑 wait_for 
          std::cout << "mutex in task get ...\n";
          cond_.wait(lock, [&]() {
              return !task.empty();  // 等待指令队列不为空
          });

          int cnt = 3;   // 3 per task 
          while (cnt-- > 0 && task.size()) {
            t.emplace_back(task.front());
            t.back()[4] = homepoint.altitude;   // 起飞点高度
            //std::cout << "task   \n\n:" << t[0] << ' ' << t[1] << std::endl;
            task.pop();	
          }
      }
      std::cout << "out of task get mutex...\n";
      // if (t.empty()) {
      //   //std::cout << "no task for now...\n";
      //     sleep(1);
      //     continue;	
      // }
      // execute task
      
      task_cnt ++;
      std::cout << "enter task: " << task_cnt << std::endl;
      // std::cout << "vector t size: " << t.size() << std::endl;
      
      // wrap to thread 
      

      if (cur_cmd) {
          // 释放资源
          if (cur_cmd == gohome)
                teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout);
          cur_cmd = none;
      }

      {
          std::lock_guard<std::mutex> lk_cmd(mutex_cmd_);
          std::swap(g_cmd, cur_cmd);
      }

      if (cur_cmd) {  // clear task queue
          std::lock_guard<std::mutex> lock(mutex_);
          std::queue<std::vector<double>> tmp;
          std::swap(tmp, task);
      }
      switch (cur_cmd) {
          case gohome:
              flightSample->goHomeAndConfirmLanding();
              break;
          case ascend:
              // reserved
              std::cout << "test " << t[0][0] << std::endl;
              runMyWaypoint(vehicle, t, responseTimeout);
              break;
          case descend:
              // reserved
              runMyWaypoint(vehicle, t, responseTimeout);
              break;
          default:
              runMyWaypoint(vehicle, t, responseTimeout);
              break;
      }
      
  }

/*
    hotptInitRadius = 10;
    runHotpointMission(vehicle, hotptInitRadius, responseTimeout);
*/
  std::cout << "end for while true" << std::endl;
  net.join();
  return 0;
}



// net thread.
void netThread() {

    std::cout << "start net thread..." << std::endl;
   //test network lry
    struct sockaddr_in server_addr;
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(7088);
    server_addr.sin_addr.s_addr = inet_addr("118.24.8.33");
    bzero(&(server_addr.sin_zero), 8);

    int server_sock_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (server_sock_fd == -1)
    {
        perror("socket error");
        //return 1;
    }
    char recv_msg[BUFFER_SIZE];
    char input_msg[BUFFER_SIZE];


    if (connect(server_sock_fd, (struct sockaddr *)&server_addr, sizeof(struct sockaddr_in)) == 0)
    {

      	std::cout << "connection sussess" << std::endl;

        // 连接成功
        fd_set client_fd_set;
        struct timeval tv;

        // 封装json并发送登陆信息
        Json::Value data;
        Json::StyledWriter sWriter;
        // data["type"] = "uva00000000001";

        std::string data_str = sWriter.write(data);
        data_str = "uva00000000001" + data_str;
        if (send(server_sock_fd, data_str.c_str(), BUFFER_SIZE, 0) == -1)
        {
              perror("发送消息出错!\n");
        }

        std::cout << "server connection seccessed." << std::endl;
        //sleep(20);
        // modify

        while (true)
        {

              // std::cout << "enter select... " << std::endl;
              tv.tv_sec = 20;
              tv.tv_usec = 0;
              FD_ZERO(&client_fd_set);
              // FD_SET(STDIN_FILENO, &client_fd_set);   // 标准输入  加入文件描述符集，让select监听输入事件 。
              FD_SET(server_sock_fd, &client_fd_set); // 服务端

              // std::cout << "blocking...\n";
              select(server_sock_fd + 1, &client_fd_set, NULL, NULL, &tv);
              // std::cout << "out block\n";
              if (FD_ISSET(STDIN_FILENO, &client_fd_set)) // 输入数据则发送
              {
                  bzero(input_msg, BUFFER_SIZE);
                  fgets(input_msg, BUFFER_SIZE, stdin);
              }
              if (FD_ISSET(server_sock_fd, &client_fd_set))
              {
                  bzero(recv_msg, BUFFER_SIZE);
                  long byte_num = recv(server_sock_fd, recv_msg, BUFFER_SIZE, 0);
                  if (byte_num > 0)
                  {
                      if (byte_num > BUFFER_SIZE)
                      {
                          byte_num = BUFFER_SIZE;
                      }
                      recv_msg[byte_num] = '\0';
                      
                      //printf("服务器:%s\n", recv_msg);
          
                      //send(server_sock_fd, recv_msg, BUFFER_SIZE, 0);

                      Json::Reader reader;// Json::Features::strictMode());
                      Json::Value parseData;

                      // 解析数据
                      // std::cout << "json parse...\n" << std::endl;
                      std::cout << recv_msg << std::endl;
                      string recv_str(recv_msg);
                      // 手动拆分，防止粘包
                      vector<string> recv_strlist = parse(recv_str);

                      for (const string & str : recv_strlist) {
                          if (!reader.parse(str, parseData))  continue; 
                          vector<double> t(5);
                          // std::cout << "json parsed success.\n";
                          // std::string aa = "json parse...";
                          
                        // send(server_sock_fd, aa.c_str(), aa.size(), 0);
                          CMD t_cmd = none;
                          string  cmd = parseData["cmd"].asString();
                          if (cmd == "gohome") {
                              t_cmd = gohome;
                          }else if (cmd == "ascend") {
                              t_cmd = ascend;
                          }else if (cmd == "descend") {
                              t_cmd = descend;
                          }

                          {
                              std::lock_guard<std::mutex> lk_cmd(mutex_cmd_);
                              g_cmd = t_cmd;
                          }
                          t[0] = t_cmd;
                          std::cout << "lry test cmd t[0]: " << t[0] << std::endl;
                          t[1] = parseData["longitude"].asDouble();
                          t[2] = parseData["latitude"].asDouble();
                          t[3] = parseData["z"].asDouble();
                          {
                              std::unique_lock<std::mutex> lock(mutex_);
                              task.push(t);
                          }
                          if (task.size() == 1) {
                              cond_.notify_one();
                          }
                          std::cout << "task size:" << task.size() << std::endl;                       
                          
                      }

                    }
                    else if (byte_num < 0)
                    {
                        printf("接受消息出错!\n");
                    }
                    else
                    {
                        printf("服务器端退出!\n");
                        net_done = true;
                        exit(0);
                    }
                }

        //sleep(1);
        } // end while(true) 
    } // end network connection
    net_done = true;
} // end thread func